﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_DataMapping.cs
 * \brief	This file contains the main function of the M44 Data Mapping functions.
 *  
 * This file contains the implementation of the M45_DataMapping function. This function
 * process the information coming from the 3DLaser Range Finder, the reconstruction of
 * the 3D environment, the gradient estimation, and assigns the occupancy and the confidence
 * factor to each of the map's cells.
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.12
 * \date      2010-02-04
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-07-01 \n
 *      First implementation of the DataMapping function.
 *      
 * - Version 0.02:   Antonio MATTA                          2009-07-09 \n
 *      Minor code added.
 *      
 * - Version 0.03:   Antonio MATTA                          2009-07-20 \n
 *      Call to function aux_MapCell added.
 *      
 * - Version 0.04:   Antonio MATTA                          2009-07-29 \n
 *      Creation of the ArrayList called CellList
 *      
 * - Version 0.05:   Antonio MATTA                          2009-08-31 \n
 *      Call to the M44_BasicClass.ConfidenceAssignment() method added.
 *      
 * - Version 0.06:   Antonio MATTA                          2009-09-03 \n
 *      Call to the M44_CreateLRM.CreateLRM(), M44_CreateLRM.LowResMap_Fusion() 
 *      methods added.
 *      
 * - Version 0.07:   Antonio MATTA                          2009-09-25 \n
 *      Call to the M44_CreateLRM.CreateLRM(), M44_CreateLRM.LowResMap_Fusion() 
 *      methods added.
 *      
 * - Version 0.08:   Antonio MATTA                          2009-09-28 \n
 *      Minor bugs fixed and documentation added.
 *      
 * - Version 0.09:  Antonio MATTA                           2009-12-14 \n
 *      DataMapping interface changed from receiving GlobalLowResMap to 
 *    _state._UpdatedCells.
 *    
 * - Version 0.10:  Antonio MATTA                           2010-01-12 \n
 *      EOS orientation paramater added to DataMapping interface.
 *      
 * - Version 0.11:  Antonio MATTA                           2010-01-13 \n
 *      Some minor fixes in order to adapt the Data Mapping to the new
 *      frame reference.
 *   
 * - Version 0.12:  Antonio MATTA                           2010-01-13 \n
 *      Updated cell eliminated from the MainDataMapping function 
 *      interface.
 *        
 * - Version 0.13: Thomas KOPFSTEDT                         2010-02-04 \n
 *   changes of class names due to cleanup in "interfaces.cs", and the 
 *   other interface classed to double names for the same class.
 *   
 * - Version 0.14: Antonio MATTA                            2010-02-26 \n
 *   _K_MAXIMUM_CONFIDENCE constant added. It sets the maximum confidence
 *   possible value.
 *   
 * - Version 0.15: Luis VALDES                               2010-04-27 \n
 *   Some variables and functions renamed
 *   Data Mapping process optmimized by eliminating intermediate variables
 *   
 * - Version 0.16: Luis VALDES                               2010-04-27 \n
 *   Functions PlotHighResMap and PlotLowResMap modified
 *   Added Function PlotMap
 */
/* ######################################################################### */

/*Autogenerated*/
using System;

/*Using the C Sharp Matrix Library*/

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*These are for plotting files*/
using System.Globalization;
using System.IO;

/*This is for using lists*/
using System.Collections;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class MainDataMapping
     *
     *  \brief  This is the main data mapping generator class.
     *
     * This class stores all the methods needed to include the data mapping
     * into the M44 module.
     * 
     */
    /* ------------------------------------------------------------------------- */
    static class MainDataMapping
    {
        #region DataMapping main function
        /* ------------------------------------------------------------------------- */
        /**	\fn public static ArrayList DataMapping( cl_RobotGPSINSState RobotEstimate,
         *                                      cl_3DLrfMeasure Lrf3DMeasure,
         *                                      cl_UpdatedCells UpdatedCells,
         *                                      cl_RobotData RobotData,
         *                                      cl_DynamicObjects DynObjectsGlobal,
         *                                      cl_DynamicObjects DetectedDynObjs,
         *                                      cl_DynamicObjects DetectedObstacles,
         *                                      cl_Map2D HighResMap,
         *                                      cl_Map2D  LowResMap,
         *                                      cl_Pos3D[,] CollisionMatrix,
         *                                      ArrayList CellList,
         *                                      cl_Orientation3D orientation,
         *                                      int MapCounter)
         * 
         *  \brief It grabs all the information needed from external classes and send them to the
         *  included methods in order to produce a Data Mapping processing.
         *  
         *  \param[in] cl_RobotGPSINSState RobotEstimate
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] cl_UpdatedCells UpdatedCells
         *  \param[in] cl_RobotData RobotData
         *  \param[in] cl_DynamicObjects DynObjectsGlobal
         *  \param[in] cl_DynamicObjects DetectedDynObjs
         *  \param[in] cl_DynamicObjects DetectedObstacles
         *  \param[in] cl_Map2D HighResMap
         *  \param[in] cl_Map2D  LowResMap
         *  \param[in] cl_Pos3D[,] CollisionMatrix
         *  \param[in] ArrayList CellList
         *  \param[in] cl_Orientation3D orientation
         *  \param[in] int MapCounter
         *  
         *  \param[out] MapList 
         *
         */
        /* ------------------------------------------------------------------------- */
        public static ArrayList DataMapping( cl_RobotGPSINSState RobotEstimate,
                                        cl_3DLrfMeasure Lrf3DMeasure,
                                        cl_RobotData RobotData,
                                        cl_DynamicObjects DynObjectsGlobal,
                                        cl_DynamicObjects DetectedDynObjs,
                                        cl_DynamicObjects DetectedObstacles,
                                        cl_Map2D HighResMap,
                                        cl_Map2D  LowResMap,
                                        cl_Pos3D[,] CollisionMatrix,
                                        ArrayList CellList,
                                        cl_Orientation3D Orientation,
                                        cl_SimulationTime SimulationTime,
                                        int MapCounter)
        {
            #region CONSTANTS
            const byte _K_MAXIMUM_CONFIDENCE = 16;
            #endregion CONSTANTS

            #region VARIABLES
            ArrayList       MapList = new ArrayList();
            cl_Map2D        High2LowRes_Map = new cl_Map2D();
            cl_SensorData   Sensor = new cl_SensorData();
            cl_EosSet       SensorData = new cl_EosSet();
            
            ArrayList[,]    Cell_List = null;
            double[,]       CartesianLocalCoord = new double[3, 1];
            double[,]       f_CartesianLocalCoord = new double[3, 1];
            double[]        CollisionPoint = new double[3];
            double[]        RobotState = new double[2];
            float[,]        tf_tmp3DLrfDistance = new float[Lrf3DMeasure.s_i, Lrf3DMeasure.s_n];
            int[]           AssignedCell = new int[2];
            int[]           RobotCell = new int[2];
            int             index_i = 0;
            int             index_j = 0;
            float           alphaMin;
            float           sigmaMax;
            float           Distance;
            ushort          s_Nrows_local = 0;
            ushort          s_Ncols_local = 0;
			float 			y = 0.0f;
            int colcounter = 0;
            

            string robotId = new string(RobotData.tc_RobotID);
            
            #endregion
            
                /*Filling Sensor Parameters*/
                foreach (cl_SensorData sensor in RobotData.Sensors)
                {
                    switch (sensor.e_SensorType)
                    {
                        /* LaserRangeFinder */
                        case E_SENSORTYPE.EOS:
                            Sensor = sensor;
                            //SensorData.tf_EosHorAngRange[0] = Sensor.SpecificData.EosSet.tf_EosHorAngRange[0];
                            SensorData.tf_EosHorAngRange[0] = -90.0f;
                            SensorData.tf_EosHorAngRange[1] = 90.0f;
                            SensorData.tf_EosHorResolution[0] = (float)(0.5f * (Math.PI / 180));
                            SensorData.tf_EosVerAngRange[0] = -45.0f;
                            SensorData.tf_EosVerAngRange[1] = 45.0f;
                            SensorData.tf_EosVerResolution[0] = (float)(0.5f * (Math.PI / 180));
                            SensorData.f_Range = 80.0f;
                            break;
                    }
                }
                
                /*Call to High Resolution Map constructor function*/
                HighResMap = M44_CreateHRM.CreateHRM(HighResMap, RobotEstimate, LowResMap, SensorData, RobotData);

                /*Auxiliary structure to save the processed information from the sensors*/
                s_Nrows_local = HighResMap.us_Nrows;
                s_Ncols_local = HighResMap.us_Ncols;
                aux_MapCell aux_MapCell_local = new aux_MapCell(s_Nrows_local, s_Ncols_local);

                float[,] Max_Heights = new float[aux_MapCell_local.us_Nrows, aux_MapCell_local.us_Ncols];

                for (int i = 0; i < s_Nrows_local; i++)
                {
                    for (int j = 0; j < s_Ncols_local; j++)
                    {
                        aux_MapCell_local.Confidence[i, j] = 0;
                        aux_MapCell_local.Occupation[i, j] = 0;
                        aux_MapCell_local.Measured_Heights[i, j] = 0.0f;
                        aux_MapCell_local.Communication_Quality[i, j] = 0;
                        aux_MapCell_local.GPS_reception_quality[i, j] = 0;
                        aux_MapCell_local.Measured_Gradient_X[i, j] = 0.0f;
                        aux_MapCell_local.Measured_Gradient_Y[i, j] = 0.0f;
                        aux_MapCell_local.Freedom_probability[i, j] = 0;
                        aux_MapCell_local.Distance[i, j] = 0.0f;
                    }
                }

                /*Sensor data processing*/
                alphaMin = (float)(SensorData.tf_EosHorAngRange[0] * (Math.PI / 180));
                sigmaMax = (float)(SensorData.tf_EosVerAngRange[1] * (Math.PI / 180));

                /*Cell_List is a cell structure for all scanned points 
                 * in every cell of the local map*/
                Cell_List = new ArrayList[s_Nrows_local, s_Ncols_local];

                /*If the robot has differential GPS, DataMapping is executed*/
                if (RobotEstimate.RobotEstimate.l_Pos_Quality > 0)
                {
                    
             /*       if (robotId == "Robot_003")
                    {
                        PlotLaser3D(Lrf3DMeasure, MapCounter, 0);
                    }
                */    

                    /*We check which cells have been explored by the sensors*/
                    for (int i = 0; i < Lrf3DMeasure.s_i; i++)
                    {
                        for (int j = 0; j < Lrf3DMeasure.s_n; j++)
                        {

                            if ((Lrf3DMeasure.tf_3DLrfDistance[i, j] != 0) && (Lrf3DMeasure.tf_3DLrfDistance[i, j] < (SensorData.f_Range - 0.1)))
                            {
                                /*The first task is to fill the ocupated cells*/
                                Distance = Lrf3DMeasure.tf_3DLrfDistance[i, j];
                                CartesianLocalCoord = M44_BasicClass.Spherical2Cartesian(Distance, sigmaMax - i * SensorData.tf_EosVerResolution[0], alphaMin + j * SensorData.tf_EosHorResolution[0]);
                                CollisionPoint = M44_BasicClass.Local2FixedCoord(CartesianLocalCoord, RobotEstimate, Sensor, Orientation);
                                cl_Pos3D CollisionPoint3D = new cl_Pos3D();
                                CollisionPoint3D.f_x = (float)Math.Round(CollisionPoint[0], 3);
                                CollisionPoint3D.f_y = (float)Math.Round(CollisionPoint[1], 3);
                                CollisionPoint3D.f_z = (float)Math.Round(CollisionPoint[2], 3);

                                /*The collision point must be stored in a matrix 
                                 * for the ObjectDetection function*/
                                CollisionMatrix[i, j] = CollisionPoint3D;

                                /*We calculate the corresponding map cell for the collision point*/
                                AssignedCell = M44_BasicClass.Coordinates2Cell_High(HighResMap, CollisionPoint);

                                /*If the cell indexes are outside the map, then break*/
                                if ((AssignedCell[0] < 0) || (AssignedCell[0] > (HighResMap.us_Nrows - 1)) || (AssignedCell[1] < 0) || (AssignedCell[1] > (HighResMap.us_Ncols - 1)))
                                {
                                    break;
                                }

                                index_i = AssignedCell[0];
                                index_j = AssignedCell[1];

                                /*Store colision point in its respective cell*/
                                if (Cell_List[index_i, index_j] == null)
                                {
                                    Cell_List[index_i, index_j] = new ArrayList();
                                    Cell_List[index_i, index_j].Add(CollisionPoint3D);
                                    colcounter = colcounter + 1;
                                }
                                else
                                {
                                    cl_Pos3D pos = new cl_Pos3D();
                                    pos = (cl_Pos3D)Cell_List[index_i, index_j][(Cell_List[index_i, index_j].Count - 1)];
                                    if (pos != CollisionPoint3D)
                                    {
                                        Cell_List[index_i, index_j].Add(CollisionPoint3D);
                                        colcounter = colcounter + 1;
                                    }

                                }

                                aux_MapCell_local.Distance[AssignedCell[0], AssignedCell[1]] = Lrf3DMeasure.tf_3DLrfDistance[i, j];
                            }
                        }
                    }
                  /*  
                    if (robotId == "Robot_001")
                    {
                        PlotCollision(Cell_List, aux_MapCell_local, MapCounter);
                        
                    }
                    */
                    
                    /*Gradient estimation*/
                    aux_MapCell_local = M44_HeightGradientEst.Height_Gradient_Estimation(Cell_List, aux_MapCell_local);
                    
                    //Max Height calculation
                    for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
                    {
                        for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                        {
                            if (Cell_List[i, j] != null)
                            {
                                /*Computation of the mean for each cell*/
                                y = 0.0f;
                                foreach (cl_Pos3D pos in Cell_List[i, j])
                                {
                                    y = Math.Max(pos.f_y, y);
                                }

                                Max_Heights[i, j] = y;
                            }
                        }
                    }


                    /*Confidence Assignmet estimation*/
                    aux_MapCell_local = M44_ConfidenceAssignmet.ConfidenceAssignment(aux_MapCell_local, SensorData.f_Range, Cell_List, HighResMap);

                  //  aux_MapCell aux_Map_Cell_local_save = aux_MapCell_local;
                  //  aux_MapCell_local.Measured_Heights = Max_Heights;
                    if (robotId == "Robot_001")
                    {
                        PlotAuxMapCell(aux_MapCell_local, MapCounter);
                    }
                   // aux_MapCell_local = aux_Map_Cell_local_save;
                    
                    
                    /*Robot Cell data filling*/
                    RobotState[0] = RobotEstimate.RobotEstimate.td_Pos[0];
                    RobotState[1] = RobotEstimate.RobotEstimate.td_Pos[1];
                    RobotCell = M44_BasicClass.Coordinates2Cell_High(HighResMap, RobotState);
                    if ((RobotCell[0] >= 0) && (RobotCell[0] <= (HighResMap.us_Nrows - 1)) && (RobotCell[1] >= 0) && (RobotCell[1] <= (HighResMap.us_Ncols - 1)))
                    {

                        aux_MapCell_local.Confidence[RobotCell[0], RobotCell[1]] = _K_MAXIMUM_CONFIDENCE;
                        aux_MapCell_local.GPS_reception_quality[RobotCell[0], RobotCell[1]] = (byte)RobotEstimate.RobotEstimate.l_Pos_Quality;
                        aux_MapCell_local.Measured_Gradient_X[RobotCell[0], RobotCell[1]] = (float)RobotEstimate.RobotEstimate.td_Attitude[0];
                        aux_MapCell_local.Measured_Gradient_Y[RobotCell[0], RobotCell[1]] = (float)RobotEstimate.RobotEstimate.td_Attitude[2];
                        aux_MapCell_local.Measured_Heights[RobotCell[0], RobotCell[1]] = (float)RobotEstimate.RobotEstimate.td_Pos[1];

                    }

                    /*Occupation Assignmet*/
                    for (int i = 0; i < s_Nrows_local; i++)
                    {
                        for (int j = 0; j < s_Ncols_local; j++)
                        {
                            if (Math.Sqrt(Math.Pow(aux_MapCell_local.Measured_Gradient_X[i, j], 2.0) + Math.Pow(aux_MapCell_local.Measured_Gradient_Y[i, j], 2.0)) > 0.3)
                            {
                                aux_MapCell_local.Occupation[i, j] = (byte)(1);
                            }
                            else
                            {
                                aux_MapCell_local.Occupation[i, j] = (byte)((int)(((1 / 0.3) * Math.Sqrt(Math.Pow(aux_MapCell_local.Measured_Gradient_X[i, j], 2.0) + Math.Pow(aux_MapCell_local.Measured_Gradient_Y[i, j], 2.0)))));
                            }
                        }
                    }

                    /*Information Fusion from temporal map to HighResMap*/
                    /*We refresh the aux_MapCell_local values with the new measures*/
                    HighResMap = M44_FusionTemp2Local.Fusion_Temporal2Local(HighResMap, aux_MapCell_local, RobotEstimate);
                    
                    /*Low resolution map Fusion*/
                    /*High2LowRes_Map is an auxiliary map with represents the same area as the HighResMap but 
                     with the same resolution of the LowResMap. It facilitates the information fusion*/
                    High2LowRes_Map = M44_CreateLRM.CreateHigh2LowMap(HighResMap, LowResMap);

                    /* Now the LowResMap is modifeid with the new information gathered by the sensors,
                     * contained in High2LowRes_Map */
                    LowResMap = M44_FusionTemp2Local.ModifyLowResMap(LowResMap, High2LowRes_Map, CellList);
                }

                /*Send output structures to plot files*/
                
                if (robotId == "Robot_001")
                {
                    PlotHighResMap(HighResMap, MapCounter, RobotEstimate, SimulationTime, Orientation);
                    //PlotLowResMap(LowResMap, MapCounter + 1, RobotEstimate, SimulationTime, Orientation);
                     PlotAttitude(RobotEstimate.RobotEstimate);
                }
                
                MapList.Add(HighResMap);
                MapList.Add(LowResMap);
                MapList.Add(CollisionMatrix);
                MapList.Add(aux_MapCell_local);
                MapList.Add(Max_Heights);

            return MapList;

        }

        #endregion

        #region DataMapping aditional functions

        #region PlotAuxMapCell
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotAuxMapCell( aux_MapCell aux_MapCell_local, int MapCounter)
         *
         *  \brief It generates a data file to plot the HighResMap values using MatLab.
         *  
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] int MapCounter
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotAuxMapCell( aux_MapCell aux_MapCell_local, int MapCounter)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            /*Plotting AuxMapCell Heights*/
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\MapCell-Heights-" + MapCounter + ".m"))
                {
                    for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
                    {
                        for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                        {
                             w.Write("{0}\t",
                                double.Parse(aux_MapCell_local.Measured_Heights[i, j].ToString()).ToString("N", nfi));
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }

            /*Plotting AuxMapCell Confidence*/
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\MapCell-Confidence-" + MapCounter + ".m"))
                {
                    for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
                    {
                        for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                        {
                            w.Write("{0}\t",
                               byte.Parse(aux_MapCell_local.Confidence[i, j].ToString()).ToString("N", nfi));
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }

        }
        #endregion 

        #region PlotCollision
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotColisionMatrix( cl_3DLrfMeasure Lrf3DMeasure, int MapCounter )
         *
         *  \brief It generates a data file to plot the HighResMap values using MatLab.
         *  
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] int MapCounter
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotCollision(ArrayList[,] Cell_List, aux_MapCell aux_MapCell_local, int MapCounter)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            /*Plotting HighResMap*/
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\Colision-" + MapCounter + ".m"))
                {
                    for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
                    {
                        for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                        {
                            if (Cell_List[i, j] != null)
                            {
                                for (int k = 0; k < Cell_List[i, j].Count; k++)
                                {
                                    cl_Pos3D pos = (cl_Pos3D)Cell_List[i, j][k];
                                    
                                    w.Write("{0}\t{1}\t{2}\n",
                                    double.Parse(pos.f_x.ToString()).ToString("N", nfi),
                                    double.Parse(pos.f_y.ToString()).ToString("N", nfi),
                                    double.Parse(pos.f_z.ToString()).ToString("N", nfi)

                                    );
                                }
                                w.Write("\n");
                            }
                        }
                    }       
                    
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }

        }
        #endregion

        #region PlotAttitude
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotAttitude(cl_RobotEstimate RobotEstimate)
         *
         *  \brief It generates a data file to plot the HighResMap values using MatLab.
         *  
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] int MapCounter
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotAttitude(cl_RobotEstimate RobotEstimate)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            /*Plotting HighResMap*/
            try
            {
                using (StreamWriter w = File.AppendText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\RobotAttitude.m"))
                {

                    w.WriteLine("{0}\t{1}\t{2}\t{3}",
                               double.Parse(RobotEstimate.f_SimulationTime.ToString()).ToString("N", nfi),
                               double.Parse(RobotEstimate.td_Attitude[0].ToString()).ToString("N", nfi),
                               double.Parse(RobotEstimate.td_Attitude[1].ToString()).ToString("N", nfi),
                               double.Parse(RobotEstimate.td_Attitude[2].ToString()).ToString("N", nfi)
                               );
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();

                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }

        }
        #endregion 

        #region PlotLaser3D
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotLaser3D(cl_3DLrfMeasure Lrf3DMeasure, int MapCounter)
         *
         *  \brief It generates a data file to plot the HighResMap values using MatLab.
         *  
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] int MapCounter
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotLaser3D(cl_3DLrfMeasure Lrf3DMeasure, int MapCounter, int type)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            string matrixtype;

            if (type == 0)
            {
                matrixtype = "No";
            }
            else
            {
                matrixtype = "Inv";
            }
            /*Plotting HighResMap*/
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\Laser3D-" + matrixtype + MapCounter + ".m"))
                {
                    for (int i = 0; i < Lrf3DMeasure.s_i; i++)
                    {
                        for (int j = 0; j < Lrf3DMeasure.s_n; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(Lrf3DMeasure.tf_3DLrfDistance[i, j].ToString()).ToString("N", nfi));
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }

        }
        #endregion

        #region PlotHighResMap
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotHighResMap(cl_Map2D HighResMap, int MapCounter, cl_RobotGPSINSState RobotEstimate,
         *                                cl_SimulationTime SimulationTime, cl_Orientation3D Orientation)
         *
         *  \brief It generates a data file to plot the HighResMap values using MatLab.
         *  
         *  \param[in] cl_Map2D HighResMap
         *  \param[in] int MapCounter
         *  \param[in] cl_RobotGPSINSState RobotEstimate
         *  \param[in] cl_SimulationTime SimulationTime
         *  \param[in] cl_Orientation3D Orientation
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotHighResMap(cl_Map2D HighResMap, int MapCounter, cl_RobotGPSINSState RobotEstimate,
                                          cl_SimulationTime SimulationTime, cl_Orientation3D Orientation)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            float f_ActualTime = SimulationTime.f_SimulationTime;
            double d_psi = Orientation.d_psi;
            double d_X = RobotEstimate.RobotEstimate.td_Pos[0];
            double d_Z = RobotEstimate.RobotEstimate.td_Pos[2];

            #region HRM_Environment
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\HRM_Env-" + MapCounter + ".txt"))
                {
                    w.Write("{0}\t{1}\t{2}\t{3}",
                        float.Parse(f_ActualTime.ToString()).ToString("N", nfi),
                        double.Parse(d_psi.ToString()).ToString("N", nfi),
                        double.Parse(d_X.ToString()).ToString("N", nfi),
                        double.Parse(d_Z.ToString()).ToString("N", nfi)
                                );

                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region HRM_Data
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\HighResMap-" + MapCounter + "-Data.txt"))
                {
                    w.Write("{0}\t{1}\t{2}\t{3}\t{4}",
                            ushort.Parse(HighResMap.us_Scale.ToString()).ToString("N", nfi),
                            ushort.Parse(HighResMap.us_Nrows.ToString()).ToString("N", nfi),
                            ushort.Parse(HighResMap.us_Ncols.ToString()).ToString("N", nfi),
                            double.Parse(HighResMap.Center.d_x.ToString()).ToString("N", nfi),
                            double.Parse(HighResMap.Center.d_y.ToString()).ToString("N", nfi)
                                );

                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Heights
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\HighResMap-Height-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < HighResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < HighResMap.us_Ncols; j++)
                        {
                            w.Write("{0}\t",
                                double.Parse(HighResMap.MapCell[i, j].f_Height.ToString()).ToString("N", nfi)
                                );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Occupation
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\HighResMap-Occupation-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < HighResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < HighResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                byte.Parse(HighResMap.MapCell[i, j].c8_Occupation.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion Occupation

            #region Gradient_x
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\HighResMap-Gradient_X-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < HighResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < HighResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(HighResMap.MapCell[i, j].f_Gradient_x.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Gradient_y
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\HighResMap-Gradient_Y-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < HighResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < HighResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(HighResMap.MapCell[i, j].f_Gradient_y.ToString()).ToString("N", nfi)
                                );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion
        }
        #endregion PlotHighResMap

        #region PlotLowResMap
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotLowResMap(cl_Map2D LowResMap, int MapCounter, cl_RobotGPSINSState RobotEstimate,
         *                                        cl_SimulationTime SimulationTime, cl_Orientation3D Orientation)
         *
         *  \brief It generates a data file to plot the LowResMap values using MatLab.
         *  
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] int MapCounter
         *  \param[in] cl_RobotGPSINSState RobotEstimate
         *  \param[in] cl_SimulationTime SimulationTime
         *  \param[in] cl_Orientation3D Orientation
         *  
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotLowResMap(cl_Map2D LowResMap, int MapCounter, cl_RobotGPSINSState RobotEstimate,
                                          cl_SimulationTime SimulationTime, cl_Orientation3D Orientation)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            float f_ActualTime = SimulationTime.f_SimulationTime;
            double d_psi = Orientation.d_psi;
            double d_X = RobotEstimate.RobotEstimate.td_Pos[0];
            double d_Z = RobotEstimate.RobotEstimate.td_Pos[2];
/*
            #region LRM_Environment
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LRM_Env-" + MapCounter + ".txt"))
                {
                    w.Write("{0}\t{1}\t{2}\t{3}",
                        float.Parse(f_ActualTime.ToString()).ToString("N", nfi),
                        double.Parse(d_psi.ToString()).ToString("N", nfi),
                        double.Parse(d_X.ToString()).ToString("N", nfi),
                        double.Parse(d_Z.ToString()).ToString("N", nfi)
                                );

                    /*Update the underlying file.*
                    w.Flush();
                    /*Close the writer and underlying file.*
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region LRM_Data
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LowResMap-=" + MapCounter + "-Data.txt"))
                {
                    w.Write("{0}\t{1}\t{2}\t{3}\t{4}",
                            ushort.Parse(LowResMap.us_Scale.ToString()).ToString("N", nfi),
                            ushort.Parse(LowResMap.us_Nrows.ToString()).ToString("N", nfi),
                            ushort.Parse(LowResMap.us_Ncols.ToString()).ToString("N", nfi),
                            double.Parse(LowResMap.Center.d_x.ToString()).ToString("N", nfi),
                            double.Parse(LowResMap.Center.d_y.ToString()).ToString("N", nfi)
                                );

                    /*Update the underlying file.*
                    w.Flush();
                    /*Close the writer and underlying file.*
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Gradient_x
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LowResMap-Gradient_X-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < LowResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < LowResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(LowResMap.MapCell[i, j].f_Gradient_x.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*
                    w.Flush();
                    /*Close the writer and underlying file.*
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Gradient_y
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LowResMap-Gradient_Y-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < LowResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < LowResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(LowResMap.MapCell[i, j].f_Gradient_y.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*
                    w.Flush();
                    /*Close the writer and underlying file.*
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion
*/
            #region Heights
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LowResMap-Height" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < LowResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < LowResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(LowResMap.MapCell[i, j].f_Height.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Occupation
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LowResMap-Occupation-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < LowResMap.us_Nrows; i++)
                    {
                        for (int j = 0; j < LowResMap.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                byte.Parse(LowResMap.MapCell[i, j].c8_Occupation.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

        }
        #endregion PlotLowResMap

        #region PlotLowResMapGlobal
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotLowResMapGlobal(cl_Map2D LowResMapGlobal, int MapCounter)
         *
         *  \brief It generates a data file to plot the LowResMap values using MatLab.
         *  
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] int MapCounter
         *  
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotLowResMapGlobal( string RobotId, cl_Map2D LowResMapGlobal, int MapCounter)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            /*Plotting LowResMap*/
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\LowResMapGlobal-" + RobotId + MapCounter + ".txt"))
                {
                    for (int i = 0; i < LowResMapGlobal.us_Nrows; i++)
                    {
                        for (int j = 0; j < LowResMapGlobal.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(LowResMapGlobal.MapCell[i, j].f_Height.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
        }
        #endregion PlotLowResMap

        #region PlotMap
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void PlotHighResMap(cl_Map2D HighResMap, int MapCounter, cl_RobotGPSINSState RobotEstimate,
         *                                cl_SimulationTime SimulationTime, cl_Orientation3D Orientation, string FileName)
         *
         *  \brief It generates a data file to plot the HighResMap values using MatLab.
         *  
         *  \param[in] cl_Map2D HighResMap
         *  \param[in] int MapCounter
         *  \param[in] cl_RobotGPSINSState RobotEstimate
         *  \param[in] cl_SimulationTime SimulationTime
         *  \param[in] cl_Orientation3D Orientation
         *  \param[in] string FileName
         */
        /* ------------------------------------------------------------------------- */
        public static void PlotMap(cl_Map2D Map, int MapCounter, cl_RobotGPSINSState RobotEstimate,
                                          cl_SimulationTime SimulationTime, cl_Orientation3D Orientation, string FileName)
        {
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberGroupSeparator = "";
            nfi.NumberDecimalDigits = 8;

            float f_ActualTime = SimulationTime.f_SimulationTime;
            double d_psi = Orientation.d_psi;
            double d_X = RobotEstimate.RobotEstimate.td_Pos[0];
            double d_Z = RobotEstimate.RobotEstimate.td_Pos[2];

            #region HRM_Environment
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\" + FileName + "-Environment" + MapCounter + ".txt"))
                {
                    w.Write("{0}\t{1}\t{2}\t{3}",
                        float.Parse(f_ActualTime.ToString()).ToString("N", nfi),
                        double.Parse(d_psi.ToString()).ToString("N", nfi),
                        double.Parse(d_X.ToString()).ToString("N", nfi),
                        double.Parse(d_Z.ToString()).ToString("N", nfi)
                                );

                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region HRM_Data
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\" + FileName + "-Data-" + MapCounter + ".txt"))
                {
                    w.Write("{0}\t{1}\t{2}\t{3}\t{4}",
                            ushort.Parse(Map.us_Scale.ToString()).ToString("N", nfi),
                            ushort.Parse(Map.us_Nrows.ToString()).ToString("N", nfi),
                            ushort.Parse(Map.us_Ncols.ToString()).ToString("N", nfi),
                            double.Parse(Map.Center.d_x.ToString()).ToString("N", nfi),
                            double.Parse(Map.Center.d_y.ToString()).ToString("N", nfi)
                                );

                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Heights
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\" + FileName + "-Height-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < Map.us_Nrows; i++)
                    {
                        for (int j = 0; j < Map.us_Ncols; j++)
                        {
                            w.Write("{0}\t",
                                double.Parse(Map.MapCell[i, j].f_Height.ToString()).ToString("N", nfi)
                                );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Occupation
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\" + FileName + "-Occupation-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < Map.us_Nrows; i++)
                    {
                        for (int j = 0; j < Map.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                byte.Parse(Map.MapCell[i, j].c8_Occupation.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion Occupation

            #region Gradient_x
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\" + FileName + "-Gradient_X-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < Map.us_Nrows; i++)
                    {
                        for (int j = 0; j < Map.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(Map.MapCell[i, j].f_Gradient_x.ToString()).ToString("N", nfi)

                        );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion

            #region Gradient_y
            try
            {
                using (StreamWriter w = File.CreateText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\Maps\\" + FileName + "-Gradient_Y-" + MapCounter + ".txt"))
                {
                    for (int i = 0; i < Map.us_Nrows; i++)
                    {
                        for (int j = 0; j < Map.us_Ncols; j++)
                        {

                            w.Write("{0}\t",
                                double.Parse(Map.MapCell[i, j].f_Gradient_y.ToString()).ToString("N", nfi)
                                );
                        }
                        w.Write("\n");
                    }
                    w.Write("\n");
                    /*Update the underlying file.*/
                    w.Flush();
                    /*Close the writer and underlying file.*/
                    w.Close();
                }
                throw new IOException();
            }
            catch (IOException e)
            {
                Console.WriteLine("{0} Caught IO exception #1.", e);
            }
            #endregion
        }
        #endregion

        #endregion
    }
        
}


/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */